/**
  ******************************************************************************
  * @file    pid.c
  * @author  FSF
  * @date    2019/9/18
  * @brief   pid面向对象风格实现
  ******************************************************************************
  * @attent  使用时先创建结构体，然后PID_StructInit，然后f_ParamInit，然后f_Calculate即可拿到output
  * @myNote  
  ******************************************************************************
  */
  
#include "pid.h"

/* --------------------------------------- Private ----------------------------------------*/
/** @brief  私有函数，但可以结构体内函数指针的方式被外部调用。功能为初始化pid参数 
  * @param  pid: 被操作的结构体，可理解为self */
static void ParamInit(
       PID_t *pid,
       float p,
       float i,
       float d,
       float integral_limit,
       float max_output)
{
    pid->p = p; 
    pid->i = i; 
    pid->d = d; 
    pid->integral_limit = integral_limit;
    pid->max_output = max_output; 
}


/** @brief  私有函数，但可以结构体内函数指针的方式被外部调用。功能为重设pid参数
  * @param  pid: 被操作的结构体，可理解为self */
static void Reset(
        PID_t *pid,
        float p,
        float i,
        float d,
        float integral_limit,
        float max_output)
{ 
    pid->p = p; 
    pid->i = i; 
    pid->d = d; 
    pid->integral_limit = integral_limit;
    pid->max_output = max_output; 
    pid->last_err = 0;
    pid->target_value = 0;
    pid->current_value = 0;
    pid->i_value = 0;
}


/** @brief  私有函数，但可以结构体内函数指针的方式被外部调用。功能为计算一帧的pid
  * @param  pid          : 被操作的结构体，可理解为self
            current_value: 该帧的当前值 */
static void Calculate(
        PID_t *pid,
        float current_value)
{
    pid->current_value = current_value;
    pid->err = pid->target_value - pid->current_value;
    float p_value = pid->p * pid->err; 
    float d_value = 0;
    if(pid->first_time_flag == 1)pid->first_time_flag = 0;  // 第一次不计算D
    else d_value = pid->d * (pid->err - pid->last_err);
    pid->i_value += pid->i * pid->err;
    if (pid->i_value > pid->integral_limit)
    {
        pid->i_value = pid->integral_limit;
    }
    if (pid->i_value < -pid->integral_limit)
    {
        pid->i_value = -pid->integral_limit;
    }
    pid->output = p_value + d_value + pid->i_value;
    pid->last_err = pid->err;
    if (pid->output > pid->max_output)
    {
        pid->output = pid->max_output;
    }
    if (pid->output < -pid->max_output)
    {
        pid->output = -pid->max_output;
    }
}

/* --------------------------------------- Public ----------------------------------------*/

/** @brief  结构体格式初始化，为输入参数结构体中的函数指针变量赋值，并初始化一些属性 */
void PID_StructInit(PID_t *pid)
{
    pid->f_ParamInit = ParamInit;
    pid->f_Reset = Reset;
    pid->f_Calculate = Calculate;
    pid->last_err = 0;
    pid->target_value = 0;
    pid->current_value = 0;
    pid->i_value = 0;
    pid->first_time_flag = 1;
}

